#!/usr/bin/env python
# Author: iris and jeff

PKG = 'sound_localization'
NAME = 'delay_detector'

import roslib; roslib.load_manifest(PKG)
import rospy
import math

from std_msgs.msg import *
from geometry_msgs.msg import *
from sound_localization.msg import SoundDelayVector


#microphones in locations in the room
MICROPHONES = [[0.0, 2.5, 0.5], \
                [0.0, -2.5, 0.5], \
                [-3.5, 0.0, 0.5], \
                [3.5, 0.0, 0.5]]

class DelayDetector():
    def __init__(self):
        rospy.init_node(NAME, anonymous=True)        
        rospy.Subscriber("/sound_localization/audio_packet", Point, self.callback)
        self.pub = rospy.Publisher('/sound_localization/sound_delay', SoundDelayVector)
        
#for simulation, receive a location (Point) of the noise
#then compute the delays vector from those points of noise

    def callback(self,data):  #data is data from audio_packet (for simulation, it's a point)
        try:
            delays_vector = SoundDelayVector() #create an instance of the class
            delays_vector.matrix_length = Int32(len(MICROPHONES))
            distances = list()
            for i in range (len(MICROPHONES)):
                src = [data.x, data.y, data.z]
                temp = self.distance(MICROPHONES[i], src)
                distances.append(temp)  #for the ith microphone, append the distance to the sound source

            for row in range (len(MICROPHONES)):  #for each mic              
                for col in range (len(MICROPHONES)): #substract its distance from the distance of each other mic
                    temp = distances[row] - distances[col]
                    delays_vector.delays_vector.append(Float64(temp))

            self.pub.publish(delays_vector)        
        except rospy.ServiceException, e:
            print "Reply failed: %s"%e


    def distance(self, src, targ):
        x = math.pow((src[0] - targ[0]), 2)
        y = math.pow((src[1] - targ[1]), 2)
        z = math.pow((src[2] - targ[2]), 2)
        return math.sqrt(x + y + z)

if __name__ == '__main__':
    try:
        st = DelayDetector()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
